/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "slam/mapping/internal/2d/local_trajectory_builder_2d.h"

#include <limits>
#include <memory>

#include "absl/memory/memory.h"
#include "slam/metrics/family_factory.h"
#include "slam/sensor/range_data.h"

namespace slam {
namespace mapping {

static auto* kLocalSlamLatencyMetric = metrics::Gauge::Null();
static auto* kLocalSlamRealTimeRatio = metrics::Gauge::Null();
static auto* kLocalSlamCpuRealTimeRatio = metrics::Gauge::Null();
static auto* kRealTimeCorrelativeScanMatcherScoreMetric =
    metrics::Histogram::Null();
static auto* kCeresScanMatcherCostMetric = metrics::Histogram::Null();
static auto* kScanMatcherResidualDistanceMetric = metrics::Histogram::Null();
static auto* kScanMatcherResidualAngleMetric = metrics::Histogram::Null();

LocalTrajectoryBuilder2D::LocalTrajectoryBuilder2D(
    const proto::LocalTrajectoryBuilderOptions2D& options,
    const std::vector<std::string>& expected_range_sensor_ids)
    : options_(options),
      active_submaps_(options.submaps_options()),
      motion_filter_(options_.motion_filter_options()),
      real_time_correlative_scan_matcher_(
          options_.real_time_correlative_scan_matcher_options()),
      ceres_scan_matcher_(options_.ceres_scan_matcher_options()),
      range_data_collator_(expected_range_sensor_ids) {
  reflector_detecor_ =
      absl::make_unique<ReflectorDetect>(options.reflector_dector_options());
}

LocalTrajectoryBuilder2D::~LocalTrajectoryBuilder2D() {}

sensor::RangeData
LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter(
    const transform::Rigid3f& transform_to_gravity_aligned_frame,
    const sensor::RangeData& range_data) const {
  const sensor::RangeData cropped =
      sensor::CropRangeData(sensor::TransformRangeData(
                                range_data, transform_to_gravity_aligned_frame),
                            options_.min_z(), options_.max_z());
  return sensor::RangeData{
      cropped.origin,
      sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.returns),
      sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.misses),
      cropped.reflectors};
}

std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
    common::Time time, const transform::Rigid2d& pose_prediction,
    const sensor::PointCloud& filtered_gravity_aligned_point_cloud,
    const sensor::PointCloud& reflectors_in_tracking,
    sensor::PointCloud* reflector_feature_in_tracking_ptr) {
  if (active_submaps_.submaps().empty()) {
    return absl::make_unique<transform::Rigid2d>(pose_prediction);
  }
  std::shared_ptr<const Submap2D> matching_submap =
      active_submaps_.submaps().front();
  // The online correlative scan matcher will refine the initial estimate for
  // the Ceres scan matcher.
  transform::Rigid2d initial_ceres_pose = pose_prediction;

  if (options_.use_online_correlative_scan_matching()) {
    const double score = real_time_correlative_scan_matcher_.Match(
        pose_prediction, filtered_gravity_aligned_point_cloud,
        *matching_submap->grid(), &initial_ceres_pose);
    kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score);
  }

  const bool has_reflector_landmark =
      ProcessReflectorData(reflectors_in_tracking, initial_ceres_pose,
                           reflector_feature_in_tracking_ptr);

  auto pose_observation = absl::make_unique<transform::Rigid2d>();
  ceres::Solver::Summary summary;
  if (has_reflector_landmark) {
    ceres_scan_matcher_.Match(
        pose_prediction.translation(), initial_ceres_pose,
        filtered_gravity_aligned_point_cloud, *matching_submap->grid(),
        local_node_id_provider_ - 1, local_node_map_,
        options_.reflector_dector_options().reflector_translation_weight(),
        &landmark_map_, pose_observation.get(), &summary);
  } else {
    ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,
                              filtered_gravity_aligned_point_cloud,
                              *matching_submap->grid(), pose_observation.get(),
                              &summary);
  }

  if (pose_observation) {
    kCeresScanMatcherCostMetric->Observe(summary.final_cost);
    const double residual_distance =
        (pose_observation->translation() - pose_prediction.translation())
            .norm();
    kScanMatcherResidualDistanceMetric->Observe(residual_distance);
    const double residual_angle =
        std::abs(pose_observation->rotation().angle() -
                 pose_prediction.rotation().angle());
    kScanMatcherResidualAngleMetric->Observe(residual_angle);
  }
  return pose_observation;
}

std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddRangeData(
    const std::string& sensor_id,
    const sensor::TimedPointCloudData& unsynchronized_data) {
  auto synchronized_data =
      range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);
  if (synchronized_data.ranges.empty()) {
    LOG(INFO) << "Range data collator filling buffer.";
    return nullptr;
  }

  const common::Time& time = synchronized_data.time;
  // Initialize extrapolator now if we do not ever use an IMU.
  if (!options_.use_imu_data()) {
    InitializeExtrapolator(time);
  }

  if (extrapolator_ == nullptr) {
    // Until we've initialized the extrapolator with our first IMU message, we
    // cannot compute the orientation of the rangefinder.
    LOG(INFO) << "Extrapolator not yet initialized.";
    return nullptr;
  }

  CHECK(!synchronized_data.ranges.empty());
  // TODO(gaschler): Check if this can strictly be 0.
  CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f);
  const common::Time time_first_point =
      time +
      common::FromSeconds(synchronized_data.ranges.front().point_time.time);
  if (time_first_point < extrapolator_->GetLastPoseTime()) {
    LOG(INFO) << "Extrapolator is still initializing.";
    return nullptr;
  }

  std::vector<transform::Rigid3f> range_data_poses;
  range_data_poses.reserve(synchronized_data.ranges.size());

  bool warned = false;
  for (const auto& range : synchronized_data.ranges) {
    common::Time time_point = time + common::FromSeconds(range.point_time.time);
    if (time_point < extrapolator_->GetLastExtrapolatedTime()) {
      if (!warned) {
        LOG(ERROR) << "Timestamp of individual range data point jumps "
                      "backwards from "
                   << extrapolator_->GetLastExtrapolatedTime() << " to "
                   << time_point;
        warned = true;
      }
      time_point = extrapolator_->GetLastExtrapolatedTime();
    }
    range_data_poses.push_back(
        extrapolator_->ExtrapolatePose(time_point).cast<float>());
  }

  if (num_accumulated_ == 0) {
    // 'accumulated_range_data_.origin' is uninitialized until the last
    // accumulation.
    accumulated_range_data_ = sensor::RangeData{{}, {}, {}};
  }
  // Drop any returns below the minimum range and convert returns beyond the
  // maximum range into misses.
  for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {
    const sensor::TimedRangefinderPoint& hit =
        synchronized_data.ranges[i].point_time;
    const Eigen::Vector3f origin_in_local =
        range_data_poses[i] *
        synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);

    sensor::RangefinderPoint hit_in_local =
        range_data_poses[i] * sensor::ToRangefinderPoint(hit);
    const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;
    const float range = delta.norm();
    if (range >= options_.min_range()) {
      if (range <= options_.max_range()) {
        accumulated_range_data_.returns.push_back(hit_in_local);
      } else {
        hit_in_local.position =
            origin_in_local +
            options_.missing_data_ray_length() / range * delta;
        accumulated_range_data_.misses.push_back(hit_in_local);
      }
    }

    if (hit_in_local.intensity >= options_.min_reflector_intensity() &&
        hit_in_local.intensity <= options_.max_reflector_intensity() &&
        range >= options_.min_reflector_range() &&
        range <= options_.max_reflector_range()) {
      accumulated_range_data_.reflectors.push_back(hit_in_local);
    }
  }

  ++num_accumulated_;

  if (num_accumulated_ >= options_.num_accumulated_range_data()) {
    const common::Time current_sensor_time = synchronized_data.time;
    absl::optional<common::Duration> sensor_duration;
    if (last_sensor_time_.has_value()) {
      sensor_duration = current_sensor_time - last_sensor_time_.value();
    }
    last_sensor_time_ = current_sensor_time;
    num_accumulated_ = 0;
    // const transform::Rigid3d gravity_alignment =
    // transform::Rigid3d::Rotation(
    // extrapolator_->EstimateGravityOrientation(time));
    const transform::Rigid3d gravity_alignment = transform::Rigid3d::Identity();
    // TODO(gaschler): This assumes that 'range_data_poses.back()' is at time
    // 'time'.
    accumulated_range_data_.origin = range_data_poses.back().translation();
    return AddAccumulatedRangeData(
        time,
        TransformToGravityAlignedFrameAndFilter(
            gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
            accumulated_range_data_),
        gravity_alignment, sensor_duration);
  }
  return nullptr;
}

std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
    const common::Time time,
    const sensor::RangeData& gravity_aligned_range_data,
    const transform::Rigid3d& gravity_alignment,
    const absl::optional<common::Duration>& sensor_duration) {
  bool point_cloud_too_less = false;
  if (gravity_aligned_range_data.returns.size() <
      options_.min_num_points_in_front()) {
    ++num_of_points_too_less_;
    point_cloud_too_less = true;
    LOG(WARNING) << "Scan points too less :  "
                 << gravity_aligned_range_data.returns.size();
    if (num_of_points_too_less_ >= 2) {
      is_laser_data_ok_ = false;
    }
  } else {
    num_of_points_too_less_ = 0;
    is_laser_data_ok_ = true;
  }

  if (gravity_aligned_range_data.returns.empty()) {
    LOG(WARNING) << "Dropped empty horizontal range data.";
    return nullptr;
  }

  transform::Rigid3d non_gravity_aligned_pose_prediction;
  transform::Rigid3d odom_pose;
  bool is_new_node = false;
  bool is_robot_still = false;
  if (options_.use_odom_in_front()) {
    if (!is_local_tracking_initialized_) {
      non_gravity_aligned_pose_prediction = transform::Rigid3d::Identity();
      transform::Rigid3d next_odom_pose;
      const bool has_next_odom =
          extrapolator_->GetPoseAtTime(time, &next_odom_pose);
      if (has_next_odom) {
        is_local_tracking_initialized_ = true;
        is_new_node = true;
      }
    } else {
      transform::Rigid3d delta_pose = transform::Rigid3d::Identity();
      transform::Rigid3d last_odom_pose;
      transform::Rigid3d next_odom_pose;
      const bool has_next_odom =
          extrapolator_->GetPoseAtTime(time, &next_odom_pose);

      const bool has_last_odom =
          extrapolator_->GetPoseAtTime(last_insert_time_, &last_odom_pose);

      odom_pose = next_odom_pose;
      if (has_last_odom && has_next_odom) {
        delta_pose = last_odom_pose.inverse() * next_odom_pose;
        if (!motion_filter_.IsSimilar(time, next_odom_pose)) {
          is_new_node = true;
        }

        if (options_.use_imu_data()) {
          Eigen::Quaterniond imu_rotation = Eigen::Quaterniond::Identity();
          bool get_rotation = extrapolator_->GetRotationBetweenTime(
              last_insert_time_, time, &imu_rotation);
          if (get_rotation) {
            delta_pose =
                transform::Rigid3d(delta_pose.translation(), imu_rotation);
#if 0
            odom_pose =
                transform::Rigid3d(odom_pose.translation(),
                                   imu_rotation * last_odom_pose.rotation());
#endif
          }
        }
        const double delta_xy = delta_pose.translation().head<2>().norm();
        const double delta_angle = std::fabs(
            common::NormalizeAngleDifference(transform::GetYaw(delta_pose)));
        is_robot_still = delta_xy < 1e-5 && delta_angle < 5e-5;
      }
      non_gravity_aligned_pose_prediction = last_pose_estimated_ * delta_pose;
    }
    extrapolator_->ExtrapolatePose(time);
  } else {
    non_gravity_aligned_pose_prediction = extrapolator_->ExtrapolatePose(time);
  }

  const transform::Rigid2d pose_prediction = transform::Project2D(
      non_gravity_aligned_pose_prediction * gravity_alignment.inverse());

  const sensor::PointCloud& filtered_gravity_aligned_point_cloud =
      sensor::AdaptiveVoxelFilter(options_.adaptive_voxel_filter_options())
          .Filter(gravity_aligned_range_data.returns);
  if (filtered_gravity_aligned_point_cloud.empty()) {
    return nullptr;
  }

  if ((options_.use_odom_in_front() && !is_new_node) ||
      only_use_odom_predict_pose_) {
    const transform::Rigid3d pose_estimate =
        transform::Embed3D(pose_prediction) * gravity_alignment;
    extrapolator_->AddPose(time, pose_estimate);

    sensor::RangeData range_data_in_local =
        TransformRangeData(gravity_aligned_range_data,
                           transform::Embed3D(pose_prediction.cast<float>()));
    std::unique_ptr<InsertionResult> insertion_result = nullptr;

    const auto wall_time = std::chrono::steady_clock::now();
    if (last_wall_time_.has_value()) {
      const auto wall_time_duration = wall_time - last_wall_time_.value();
      kLocalSlamLatencyMetric->Set(common::ToSeconds(wall_time_duration));
      if (sensor_duration.has_value()) {
        kLocalSlamRealTimeRatio->Set(
            common::ToSeconds(sensor_duration.value()) /
            common::ToSeconds(wall_time_duration));
      }
    }
    const double thread_cpu_time_seconds = common::GetThreadCpuTimeSeconds();
    if (last_thread_cpu_time_seconds_.has_value()) {
      const double thread_cpu_duration_seconds =
          thread_cpu_time_seconds - last_thread_cpu_time_seconds_.value();
      if (sensor_duration.has_value()) {
        kLocalSlamCpuRealTimeRatio->Set(
            common::ToSeconds(sensor_duration.value()) /
            thread_cpu_duration_seconds);
      }
    }
    last_wall_time_ = wall_time;
    last_thread_cpu_time_seconds_ = thread_cpu_time_seconds;
    return absl::make_unique<MatchingResult>(MatchingResult{
        time, pose_estimate,
        options_.use_odom_in_front() ? odom_pose : pose_estimate,
        std::move(range_data_in_local), std::move(insertion_result)});
  }

  sensor::PointCloud reflector_feature_in_tracking;
  std::unique_ptr<transform::Rigid2d> pose_estimate_2d = ScanMatch(
      time, pose_prediction, filtered_gravity_aligned_point_cloud,
      gravity_aligned_range_data.reflectors, &reflector_feature_in_tracking);

  if (pose_estimate_2d == nullptr) {
    LOG(WARNING) << "Scan matching failed.";
    return nullptr;
  }

  const transform::Rigid2d pose_diff =
      pose_prediction.inverse() * (*pose_estimate_2d);

  if (options_.use_odom_in_front()) {
    if (is_robot_still || point_cloud_too_less) {
      *pose_estimate_2d = transform::Rigid2d(pose_prediction.translation(),
                                             pose_prediction.rotation());
    } else {
      const double delta_translation = pose_diff.translation().norm();
      const double delta_rotation =
          common::NormalizeAngleDifference(pose_diff.rotation().angle());
      if (delta_translation > 0.1 || delta_rotation > 0.05) {
        *pose_estimate_2d = transform::Rigid2d(pose_prediction.translation(),
                                               pose_prediction.rotation());
        LOG(WARNING) << "The pose estimation is too diff, " << pose_diff;
      }
    }
  }

  const transform::Rigid3d pose_estimate =
      transform::Embed3D(*pose_estimate_2d) * gravity_alignment;
  extrapolator_->AddPose(time, pose_estimate);
  if (options_.use_odom_in_front()) {
    last_insert_time_ = time;
    last_pose_estimated_ = pose_estimate;
    extrapolator_->RemoveOdometryBeforTime(last_insert_time_ -
                                           common::FromSeconds(5));
    if (options_.use_imu_data()) {
      extrapolator_->RemoveImuBeforTime(last_insert_time_ -
                                        common::FromSeconds(5));
    }
  }

  sensor::RangeData range_data_in_local =
      TransformRangeData(gravity_aligned_range_data,
                         transform::Embed3D(pose_estimate_2d->cast<float>()));
  std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
      time, gravity_aligned_range_data, reflector_feature_in_tracking,
      range_data_in_local, filtered_gravity_aligned_point_cloud, pose_estimate,
      gravity_alignment.rotation());

  const auto wall_time = std::chrono::steady_clock::now();
  if (last_wall_time_.has_value()) {
    const auto wall_time_duration = wall_time - last_wall_time_.value();
    kLocalSlamLatencyMetric->Set(common::ToSeconds(wall_time_duration));
    if (sensor_duration.has_value()) {
      kLocalSlamRealTimeRatio->Set(common::ToSeconds(sensor_duration.value()) /
                                   common::ToSeconds(wall_time_duration));
    }
  }
  const double thread_cpu_time_seconds = common::GetThreadCpuTimeSeconds();
  if (last_thread_cpu_time_seconds_.has_value()) {
    const double thread_cpu_duration_seconds =
        thread_cpu_time_seconds - last_thread_cpu_time_seconds_.value();
    if (sensor_duration.has_value()) {
      kLocalSlamCpuRealTimeRatio->Set(
          common::ToSeconds(sensor_duration.value()) /
          thread_cpu_duration_seconds);
    }
  }
  last_wall_time_ = wall_time;
  last_thread_cpu_time_seconds_ = thread_cpu_time_seconds;
  return absl::make_unique<MatchingResult>(MatchingResult{
      time, pose_estimate,
      options_.use_odom_in_front() ? odom_pose : pose_estimate,
      std::move(range_data_in_local), std::move(insertion_result)});
}

std::unique_ptr<LocalTrajectoryBuilder2D::InsertionResult>
LocalTrajectoryBuilder2D::InsertIntoSubmap(
    const common::Time time, const sensor::RangeData& range_data_in_tracking,
    const sensor::PointCloud& reflector_feature_in_tracking,
    const sensor::RangeData& range_data_in_local,
    const sensor::PointCloud& filtered_gravity_aligned_point_cloud,
    const transform::Rigid3d& pose_estimate,
    const Eigen::Quaterniond& gravity_alignment) {
  if (!options_.use_odom_in_front() &&
      motion_filter_.IsSimilar(time, pose_estimate)) {
    return nullptr;
  }
  std::vector<std::shared_ptr<const Submap2D>> insertion_submaps =
      active_submaps_.InsertRangeData(range_data_in_local);
  return absl::make_unique<InsertionResult>(InsertionResult{
      std::make_shared<const TrajectoryNode::Data>(TrajectoryNode::Data{
          time,
          gravity_alignment,
          filtered_gravity_aligned_point_cloud,
          {},  // 'high_resolution_point_cloud' is only used in 3D.
          {},  // 'low_resolution_point_cloud' is only used in 3D.
          {},  // 'rotational_scan_matcher_histogram' is only used in 3D.
          pose_estimate,
          range_data_in_tracking,
          reflector_feature_in_tracking}),
      std::move(insertion_submaps)});
}

void LocalTrajectoryBuilder2D::AddImuData(const sensor::ImuData& imu_data) {
  CHECK(options_.use_imu_data()) << "An unexpected IMU packet was added.";
  InitializeExtrapolator(imu_data.time);
  extrapolator_->AddImuData(imu_data);
  if (options_.use_odom_in_front()) {
    extrapolator_->AddImu(imu_data);
  }
}

void LocalTrajectoryBuilder2D::AddOdometryData(
    const sensor::OdometryData& odometry_data) {
  if (extrapolator_ == nullptr) {
    // Until we've initialized the extrapolator we cannot add odometry data.
    LOG(INFO) << "Extrapolator not yet initialized.";
    return;
  }
  extrapolator_->AddOdometryData(odometry_data);
  if (options_.use_odom_in_front()) {
    extrapolator_->AddOdometry(odometry_data);
  }
}

void LocalTrajectoryBuilder2D::InitializeExtrapolator(const common::Time time) {
  if (extrapolator_ != nullptr) {
    return;
  }
  CHECK(!options_.pose_extrapolator_options().use_imu_based());
  // TODO(gaschler): Consider using InitializeWithImu as 3D does.
  extrapolator_ = absl::make_unique<PoseExtrapolator>(
      ::slam::common::FromSeconds(options_.pose_extrapolator_options()
                                      .constant_velocity()
                                      .pose_queue_duration()),
      options_.pose_extrapolator_options()
          .constant_velocity()
          .imu_gravity_time_constant());
  LOG(INFO) << "Initialize pose extrapolator!!!";
  extrapolator_->AddPose(time, transform::Rigid3d::Identity());
}

bool LocalTrajectoryBuilder2D::ProcessReflectorData(
    const sensor::PointCloud& reflectors_in_tracking,
    const transform::Rigid2d& T_local_tracking_predict,
    sensor::PointCloud* reflector_feature_in_tracking_ptr) {
  bool has_reflector_landmark = false;
  sensor::PointCloud& reflector_feature_in_tracking =
      *reflector_feature_in_tracking_ptr;
  if (!options_.use_reflector() || reflectors_in_tracking.size() < 1) {
    return false;
  }

  LocalNodePtr local_node = std::shared_ptr<LocalNode>(new LocalNode());
  local_node->node_id = local_node_id_provider_++;
  local_node->T_local_tracking = T_local_tracking_predict;

  reflector_detecor_->ExtractReflectorFeature(reflectors_in_tracking,
                                              &reflector_feature_in_tracking);
  std::vector<int> match_reflector_ids;
  std::vector<double> match_min_dists;
  reflector_detecor_->ReflectorMatching(landmark_map_, T_local_tracking_predict,
                                        reflector_feature_in_tracking,
                                        &match_reflector_ids, &match_min_dists);

  std::set<int> cur_observed_landmark_id;
  for (size_t i = 0; i < reflector_feature_in_tracking.size(); ++i) {
    int match_reflector_id = match_reflector_ids[i];
    if (match_reflector_id < 0 &&
        (match_min_dists[i] < options_.reflector_dector_options()
                                  .new_reflector_min_distance_threshold())) {
      continue;
    }
    const Eigen::Vector2d p_tracking_landmark =
        reflector_feature_in_tracking[i].position.head<2>().cast<double>();
    if (match_reflector_id < 0) {
      match_reflector_id = landmark_id_provider_++;
      LandmarkPtr landmark = std::shared_ptr<Landmark>(new Landmark());
      landmark->landmark_id = match_reflector_id;
      landmark->p_local_landmark =
          T_local_tracking_predict * p_tracking_landmark;

      landmark_map_[match_reflector_id] = landmark;
    } else {
      has_reflector_landmark = true;
    }

    CHECK(landmark_map_.count(match_reflector_id) == 1);
    auto& landmark = landmark_map_[match_reflector_id];

    LandmarkObservation observation;
    observation.node_id = local_node->node_id;
    observation.landmark_id = match_reflector_id;
    observation.p_tracking_landmark = p_tracking_landmark;

    local_node->observations.emplace_back(observation);

    landmark->num_node_not_observed_ = 0;
    landmark->observations.emplace_back(observation);

    cur_observed_landmark_id.insert(match_reflector_id);
  }

  local_node_map_[local_node->node_id] = local_node;
  for (const auto& kv : landmark_map_) {
    const auto p_local_landmark = kv.second->p_local_landmark;
    if (cur_observed_landmark_id.count(kv.first) == 0) {
      kv.second->num_node_not_observed_++;
    }
  }

  auto it = landmark_map_.begin();
  while (it != landmark_map_.end()) {
    if (it->second->num_node_not_observed_ > 3) {
      landmark_map_.erase(it++);
    } else {
      it++;
    }
  }

  auto iter = local_node_map_.begin();
  while (local_node_map_.size() > 999) {
    local_node_map_.erase(iter, std::next(iter, 100));
  }
  return has_reflector_landmark;
}

void LocalTrajectoryBuilder2D::SetOnlyUseOdomPredictPoseFlag(const bool flag) {
  if (only_use_odom_predict_pose_ != flag) {
    only_use_odom_predict_pose_ = flag;
    LOG(WARNING) << "Switch only_use_odom_predict_pose to "
                 << only_use_odom_predict_pose_;
  }
}

bool LocalTrajectoryBuilder2D::QueryFontLaserDataStatus() {
  return is_laser_data_ok_;
}

void LocalTrajectoryBuilder2D::RegisterMetrics(
    metrics::FamilyFactory* family_factory) {
  auto* latency = family_factory->NewGaugeFamily(
      "mapping_2d_local_trajectory_builder_latency",
      "Duration from first incoming point cloud in accumulation to local slam "
      "result");
  kLocalSlamLatencyMetric = latency->Add({});
  auto* real_time_ratio = family_factory->NewGaugeFamily(
      "mapping_2d_local_trajectory_builder_real_time_ratio",
      "sensor duration / wall clock duration.");
  kLocalSlamRealTimeRatio = real_time_ratio->Add({});

  auto* cpu_real_time_ratio = family_factory->NewGaugeFamily(
      "mapping_2d_local_trajectory_builder_cpu_real_time_ratio",
      "sensor duration / cpu duration.");
  kLocalSlamCpuRealTimeRatio = cpu_real_time_ratio->Add({});
  auto score_boundaries = metrics::Histogram::FixedWidth(0.05, 20);
  auto* scores = family_factory->NewHistogramFamily(
      "mapping_2d_local_trajectory_builder_scores", "Local scan matcher scores",
      score_boundaries);
  kRealTimeCorrelativeScanMatcherScoreMetric =
      scores->Add({{"scan_matcher", "real_time_correlative"}});
  auto cost_boundaries = metrics::Histogram::ScaledPowersOf(2, 0.01, 100);
  auto* costs = family_factory->NewHistogramFamily(
      "mapping_2d_local_trajectory_builder_costs", "Local scan matcher costs",
      cost_boundaries);
  kCeresScanMatcherCostMetric = costs->Add({{"scan_matcher", "ceres"}});
  auto distance_boundaries = metrics::Histogram::ScaledPowersOf(2, 0.01, 10);
  auto* residuals = family_factory->NewHistogramFamily(
      "mapping_2d_local_trajectory_builder_residuals",
      "Local scan matcher residuals", distance_boundaries);
  kScanMatcherResidualDistanceMetric =
      residuals->Add({{"component", "distance"}});
  kScanMatcherResidualAngleMetric = residuals->Add({{"component", "angle"}});
}

}  // namespace mapping
}  // namespace slam
